#include "file_operator.h"
#include <cmath>
imu_data imu_average(std::vector<imu_data> &imuList){
    imu_data tmp,average;
    tmp.ax=0;
    tmp.ay=0;
    tmp.az=0;
    tmp.gx=0;
    tmp.gy=0;
    tmp.gz=0;
    for (int i = 0; i <imuList.size(); i += 1)
    {
        tmp.ax =tmp.ax+imuList[i].ax;
        tmp.ay =tmp.ay+imuList[i].ay;
        tmp.az =tmp.az+imuList[i].az;

        tmp.gx =tmp.gx+imuList[i].gx;
        tmp.gy =tmp.gy+imuList[i].gy;
        tmp.gz =tmp.gz+imuList[i].gz;
    }
    average.ax=tmp.ax/(imuList.size());
    average.ay=tmp.ay/(imuList.size());
    average.az=tmp.az/(imuList.size());
    average.gx=tmp.gx/(imuList.size());
    average.gy=tmp.gy/(imuList.size());
    average.gz=tmp.gz/(imuList.size());
    return average;
}

imu_data bias_count(std::vector<imu_data> &imuList,std::vector<imu_data> &imuList1){
    imu_data bias;
    bias.ax = (imu_average(imuList).ax+imu_average(imuList1).ax)/2;
    bias.ay = (imu_average(imuList).ay+imu_average(imuList1).ay)/2;
    bias.az = (imu_average(imuList).az+imu_average(imuList1).az)/2;
    bias.gx = (imu_average(imuList).gx+imu_average(imuList1).gx)/2;
    bias.gy = (imu_average(imuList).gy+imu_average(imuList1).gy)/2;
    bias.gz = (imu_average(imuList).gz+imu_average(imuList1).gz)/2;
    return bias;
}

imu_data imu_varience(const imu_data &imu_average,std::vector<imu_data> &tmp_bias){
    imu_data tmp;
    tmp.ax=0;
    tmp.ay=0;
    tmp.az=0;
    tmp.gx=0;
    tmp.gy=0;
    tmp.gz=0;
    for(int i=0;i<tmp_bias.size();i++){
        tmp.ax = tmp.ax+pow(tmp_bias[i].ax-imu_average.ax,2);
        tmp.ay = tmp.ax+pow(tmp_bias[i].ay-imu_average.ay,2);
        tmp.az = tmp.az+pow(tmp_bias[i].az-imu_average.az,2);

        tmp.gx = tmp.gx+pow(tmp_bias[i].gx-imu_average.gx,2);
        tmp.gy = tmp.gx+pow(tmp_bias[i].gy-imu_average.gy,2);
        tmp.gz = tmp.gz+pow(tmp_bias[i].gz-imu_average.gz,2);
    }
    tmp.ax = sqrt(tmp.ax/tmp_bias.size());
    tmp.ay = sqrt(tmp.ay/tmp_bias.size());
    tmp.az = sqrt(tmp.az/tmp_bias.size());
    tmp.gx = sqrt(tmp.gx/tmp_bias.size());
    tmp.gy = sqrt(tmp.gy/tmp_bias.size());
    tmp.gz = sqrt(tmp.gz/tmp_bias.size());
    return tmp;
}